#include <Arduino.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <WiFiClientSecure.h>
// const char *ssid = "HUAWEI-CS1V7D";      // Change this to your WiFi SSID
// const char *password = "yong10130801.."; // Change this to your WiFi password

// WiFi settings
const char *ssid = "zxg";          // Replace with your WiFi name
const char *password = "12345678"; // Replace with your WiFi password

// MQTT Broker settings
const char *mqtt_broker = "p5a17eee.ala.cn-hangzhou.emqxsl.cn"; // EMQX broker endpoint
const char *mqtt_username = "zxg";                              // MQTT username for authentication
const char *mqtt_password = "zxg123456";                        // MQTT password for authentication
const int mqtt_port = 8883;                                     // MQTT port (TCP)

// DC Motor settings
const int B_1A = 32; //
const int B_1B = 33; //
const int A_1A = 25; //
const int A_1B = 26; //

int motorStatus = 0;    // 0 关闭 1 开启
int forwardAndBack = 1; // 1 向前 2 向后
int leftAndRight = 0;   // 0居中 1向左 2向右

WiFiClientSecure espClient;
PubSubClient mqtt_client(espClient);

void connectToWiFi();
void connectToMQTTBroker();

void mqttCallback(char *topic, byte *payload, unsigned int length);

void setup()
{
  Serial.begin(115200);
  connectToWiFi();
  pinMode(2, OUTPUT);

  // mqtt init
  espClient.setInsecure();
  mqtt_client.setServer(mqtt_broker, mqtt_port);
  mqtt_client.setCallback(mqttCallback);
  connectToMQTTBroker();

  pinMode(B_1A, OUTPUT);
  pinMode(B_1B, OUTPUT);
  pinMode(A_1A, OUTPUT);
  pinMode(A_1B, OUTPUT);

  digitalWrite(B_1A, LOW);
  digitalWrite(B_1B, LOW);
  digitalWrite(A_1A, LOW);
  digitalWrite(A_1B, LOW);
}

void loop()
{
  if (WiFi.status() == WL_CONNECTED)
  {
    digitalWrite(2, HIGH);
    if (!mqtt_client.connected())
    {
      connectToMQTTBroker();
    }
  }
  else
  {
    WiFi.begin(ssid, password);
    digitalWrite(2, HIGH);
    delay(1000);
    digitalWrite(2, LOW);
  }
  mqtt_client.loop();
  delay(1000);
}

void connectToWiFi()
{
  WiFi.setHostname("zxg_esp32");
  WiFi.begin(ssid, password);
  Serial.print("Connecting to WiFi");
  while (WiFi.status() != WL_CONNECTED)
  {
    digitalWrite(2, HIGH);
    Serial.print(".");
    delay(1000);
    digitalWrite(2, LOW);
    delay(1000);
  }

  Serial.println(WiFi.getHostname());
  Serial.println("\nConnected to the WiFi network");
}

void connectToMQTTBroker()
{
  while (!mqtt_client.connected())
  {
    String client_id = "esp32-client-" + WiFi.macAddress();
    Serial.printf("Connecting to MQTT Broker as %s.....\n", client_id.c_str());
    if (mqtt_client.connect(client_id.c_str(), mqtt_username, mqtt_password))
    {
      Serial.println("Connected to MQTT broker");
      mqtt_client.subscribe(String("emqx/esp32/" + WiFi.macAddress()).c_str());
      // Publish message upon successful connection
      mqtt_client.publish("emqx/esp32/status", WiFi.macAddress().c_str());
    }
    else
    {
      Serial.print("Failed to connect to MQTT broker, rc=");
      Serial.print(mqtt_client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void mqttCallback(char *topic, byte *payload, unsigned int length)
{
  // 将payload转换为字符串
  String message = String((char *)payload).substring(0, length);

  // 使用strcmp()比较字符串
  if (strcmp(message.c_str(), "forward") == 0)
  {
    Serial.println("forward");

    digitalWrite(B_1A, LOW);
    digitalWrite(B_1B, LOW);
    digitalWrite(A_1A, LOW);
    digitalWrite(A_1B, LOW);
    delay(50);
    forwardAndBack = 1;
    leftAndRight = 0;
    digitalWrite(B_1A, HIGH);
    digitalWrite(B_1B, LOW);
    digitalWrite(A_1A, HIGH);
    digitalWrite(A_1B, LOW);
  }
  else if (strcmp(message.c_str(), "back") == 0)
  {
    Serial.println("back");
    digitalWrite(B_1A, LOW);
    digitalWrite(B_1B, LOW);
    digitalWrite(A_1A, LOW);
    digitalWrite(A_1B, LOW);
    delay(50);
    forwardAndBack = 2;
    leftAndRight = 0;
    digitalWrite(B_1A, LOW);
    digitalWrite(B_1B, HIGH);
    digitalWrite(A_1A, LOW);
    digitalWrite(A_1B, HIGH);
  }
  else if (strcmp(message.c_str(), "left") == 0)
  {
    Serial.println("left");
  }
  else if (strcmp(message.c_str(), "right") == 0)
  {
    Serial.println("right");
  }
  else if (strcmp(message.c_str(), "stop") == 0)
  {
    digitalWrite(B_1A, LOW);
    digitalWrite(B_1B, LOW);
    digitalWrite(A_1A, LOW);
    digitalWrite(A_1B, LOW);
  }
  else
  {
    // 如果字符串不相等，执行其他操作或什么都不做
    Serial.println("Received unexpected message");
  }
}